/**
 * @file race_tasks.c
 * @author bupt 20th
 * @brief TR比赛任务相关
 * @version 0.1
 * @date 2021-07-21
 * 
 * @copyright Copyright (c) 2021
 * 
 */

/**
 * 几点说明：
 * ① 底盘任务和其他任务采用全局变量通信
 * ② 取箭任务和射箭任务，以及取箭、射箭任务和其他任务之间采用信号量及事件组进行通信
 * ③ 比赛任务状态机内部之间采用结构体变量通信
 * ④ 到达取箭点分两步：先前往箭架点1，进行激光校准，再前往箭架点2，开始取箭
 */

#include "race_tasks.h"
#include "base_chassis.h"
#include "rudder_chassis.h"
#include "FreeRTOS.h"
#include "laser.h"
#include "shooter.h"
#include "oslib_uart.h"
#include "claw_signal.h"

/* 地图关键点参数 */
const Point2D ArrowFramePoint1 = {-10.9400, 0.830000};
const Point2D ArrowFramePoint2 = {-10.917, 0.708}; // 能取到箭时的点，x是怼紧墙时的读数
const Point2D ShootPoint1 = {-10.291, 1.182};
const Point2D ShootPoint2 = {-10.511, 4.180};
const float ArrowFramePoint1_LaserDist = 810.5979; // 箭架点1时的激光读数

/* 全局变量----------------------------------------------------*/
RaceStateMachine_s RaceStateMachine = {0};
extern osEventFlagsId_t EventRaceStateTransHandle;

void RaceTasks_StateMachine_Init(void)
{
    RaceStateMachine.state = RS_NONE;
    RaceStateMachine.event = &EventRaceStateTransHandle;
    RaceStateMachine.shoot_point_arrived_count = 0;
    RaceStateMachine.hitted_wall = 0;
    RaceStateMachine.arrow_shooted = 0;
    RaceStateMachine.arrow_blank = 0;
}

/**
 * @brief 比赛任务状态机执行函数
 * 
 */
void RaceTacks_StateMachine_Exe(void)
{
    switch (RaceStateMachine.state)
    {
    case RS_NONE:
        /* code */
        break;
    // ------------------------------------------------------------
    case RS_AT_HOME: // 启动区射两只箭
        // ... 等待箭空.阻塞在此处
        OSLIB_Uart_Printf(&huart1, "RSM|going to arrow frame...\r\n");
        RaceStateMachine.state = RS_GO_TO_ARROW_FRAME_FROM_START;
        RaceStateMachine.target_point_index = RKP_ARROW_FRAME1;
        Chassis_TrackStatus_Reset();
        RudderChassis.base->ctrl_mode = CTRL_MODE_TRACK;
        RudderChassis.base->TrackStatus.track_path_index = 1;
        RudderChassis.base->TrackStatus.start = 1;
        break;
    // ------------------------------------------------------------
    case RS_GO_TO_ARROW_FRAME_FROM_START:
        RaceTask_JudgeHittedWall();
        if (!RaceStateMachine.arrived_arrow_frame_point1)
        {
            // 等待到达
            if (RudderChassis.base->TrackStatus.finished) // 到达箭架点1的瞬态，开始以go2point模式到达最终取箭点
            {
                Chassis_TrackStatus_Reset();
                Chassis_Go2Point_Reset();
                OSLIB_Uart_Printf(&huart1, "RSM|arrived at arrow frame p1...");
                RaceStateMachine.arrived_arrow_frame_point1 = 1; // 跳转进else
                // 获取激光修正量
                osDelay(100);
                float laser_delta_y = (ArrowFramePoint1_LaserDist - laser_get_dist(chassis_laser1)) / 1000.0f;
                RudderChassis.base->PostureStatus.pos_corr_y = -laser_delta_y;
                uprintf("RSM|laser_delta_y:%.5f \r\n", laser_delta_y);

                RaceStateMachine.target_point_index = RKP_ARROW_FRAME2;
                RudderChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT; // 以纯跑点模式前往箭架点2
                RudderChassis.base->Go2PointStatus.start_speed = 0.35;
                RudderChassis.base->Go2PointStatus.final_speed = 0;
                RudderChassis.base->Go2PointStatus.target_point.x = ArrowFramePoint2.x;
                RudderChassis.base->Go2PointStatus.target_point.y = ArrowFramePoint2.y;
                RudderChassis.base->Go2PointStatus.target_yaw = 1.5708;
                RudderChassis.base->Go2PointStatus.disable_yaw_ctrl = 1;
                RudderChassis.base->Go2PointStatus.start = 1;
                RudderChassis.base->Go2PointStatus.enable = 1;
            }
        }
        else
        {
            // uprintf("RSM|Going to arrow frame point2...\r\n");
            // 阻塞等待到达
            if (RudderChassis.base->Go2PointStatus.arrived) // 到达取箭点2的瞬态
            {
                OSLIB_Uart_Printf(&huart1, "RSM|arrived at arrow frame p2...\r\n");
                Chassis_Go2Point_Reset();
                RaceStateMachine.hitted_wall = 0;
                RaceStateMachine.arrived_arrow_frame_point1 = 0;
                RaceStateMachine.arrived_arrow_frame_point2 = 1;

                // RaceStateMachine.state = RS_NONE;

                RaceStateMachine.target_point_index = RKP_SHOOT_POINT1; // 测试用
                RaceStateMachine.state = RS_FETCHING_ARROW;
                uprintf("RSM|aser_dis:%.4f\r\n", laser_get_dist(chassis_laser1));
                Signal_StartTakeArrow(); // 调用取箭函数
            }
        }
        break;
    // ------------------------------------------------------------
    case RS_FETCHING_ARROW:
        OSLIB_Uart_Printf(&huart1, "RSM|waiting for fetch arrow...\r\n");
        if (RaceStateMachine.fetch_arrow_ok) // 瞬态
        {
            RaceStateMachine.fetch_arrow_ok = 0;
            RaceStateMachine.arrived_arrow_frame_point2 = 0;

            OSLIB_Uart_Printf(&huart1, "RSM|Fetching arrow ok. Going to arrow frame p1...\r\n");
            RaceStateMachine.state = RS_GOING_TO_ARROW_FRAME_POINT1_AFTER_FETCH;
            RudderChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
            RudderChassis.base->Go2PointStatus.target_point.x = ArrowFramePoint2.x + 0.01; //此时不用怼着墙
            RudderChassis.base->Go2PointStatus.target_point.y = ArrowFramePoint1.y;
            RudderChassis.base->Go2PointStatus.target_yaw = 1.5708;
            RudderChassis.base->Go2PointStatus.disable_yaw_ctrl = 0;
            RudderChassis.base->Go2PointStatus.start_speed = 0.5;
            RudderChassis.base->Go2PointStatus.final_speed = 0.1;
            RudderChassis.base->Go2PointStatus.start = 1;
            RudderChassis.base->Go2PointStatus.enable = 1;

            uprintf("RSM|shoot_point_arrived_count:%d\r\n", RaceStateMachine.shoot_point_arrived_count);
            if (RaceStateMachine.shoot_point_arrived_count <= 2)
            {
                RaceStateMachine.target_point_index = RKP_SHOOT_POINT1; // 测试用
            }
            else if (RaceStateMachine.shoot_point_arrived_count <= 4)
            {
                RaceStateMachine.target_point_index = RKP_SHOOT_POINT2; // 测试用
            }
            else
            {
                RaceStateMachine.target_point_index = RKP_NONE;
                RaceStateMachine.state = RS_NONE;
                uprintf("RSM|Race ended!\r\n");
            }
        }
        break;
    // ------------------------------------------------------------
    case RS_GOING_TO_ARROW_FRAME_POINT1_AFTER_FETCH:
        if (RudderChassis.base->Go2PointStatus.arrived) // 到达
        {
            Chassis_Go2Point_Reset();
            OSLIB_Uart_Printf(&huart1, "RSM|fetch arrow ok, waiting for target_point...\r\n");
            // >>>将state置位时就应指定目标点<<<
            switch (RaceStateMachine.target_point_index) // 从箭架出发有多种路径可选
            {
            case RKP_NONE:
                RudderChassis.base->ctrl_mode = CTRL_MODE_NONE;
                break;
            case RKP_SHOOT_POINT1:
                OSLIB_Uart_Printf(&huart1, "RSM|going to shoot point1...\r\n");
                RaceStateMachine.state = RS_GOING_TO_SHOOT_POINT1;
                RudderChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
                RudderChassis.base->Go2PointStatus.target_point.x = ShootPoint1.x;
                RudderChassis.base->Go2PointStatus.target_point.y = ShootPoint1.y;
                RudderChassis.base->Go2PointStatus.target_yaw = 0.785;
                RudderChassis.base->Go2PointStatus.start_speed = 0.5;
                RudderChassis.base->Go2PointStatus.final_speed = 0;
                RudderChassis.base->Go2PointStatus.disable_yaw_ctrl = 0;
                RudderChassis.base->Go2PointStatus.enable_always_yaw_ctrl = 0;
                RudderChassis.base->Go2PointStatus.start = 1;
                RudderChassis.base->Go2PointStatus.enable = 1;
                
                break;
            case RKP_SHOOT_POINT2:
                OSLIB_Uart_Printf(&huart1, "RSM|going to shoot point2...\r\n");
                RaceStateMachine.state = RS_GOING_TO_SHOOT_POINT2;
                Chassis_Go2Point_Reset();
                RudderChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
                RudderChassis.base->Go2PointStatus.target_point.x = ShootPoint2.x;
                RudderChassis.base->Go2PointStatus.target_point.y = ShootPoint2.y;
                RudderChassis.base->Go2PointStatus.target_yaw = 0.785;
                RudderChassis.base->Go2PointStatus.start_speed = 0.5;
                RudderChassis.base->Go2PointStatus.final_speed = 0;
                RudderChassis.base->Go2PointStatus.enable_always_yaw_ctrl = 0;
                RudderChassis.base->Go2PointStatus.disable_yaw_ctrl = 0;
                RudderChassis.base->Go2PointStatus.start = 1;
                RudderChassis.base->Go2PointStatus.enable = 1;
                break;
            default:
                OSLIB_Uart_Printf(&huart1, "RSM | ## target point error! ## state <RS_FETCHING_ARROW>\r\n");
                break;
            }
        }
        break;
    // ------------------------------------------------------------
    case RS_GOING_TO_SHOOT_POINT1:
        if (!RaceStateMachine.arrived_shoot_point1)
        {
            if (RudderChassis.base->Go2PointStatus.arrived) // 瞬态，射箭点1只可能从箭架前来，无需考虑track的情况
            {
                uprintf("RSM|Arrived at shoot point1.\r\n");
                RaceStateMachine.arrived_shoot_point1 = 1;
                Chassis_Go2Point_Reset();
                RaceStateMachine.shoot_point_arrived_count++;
                shooter_set_point(shooter1, 1); // 设置为射箭点1的拟合参数
            }
        }
        else // 常态，到达射箭点1
        {
            // 等待箭空且最后一只箭已射出
            if (RaceStateMachine.arrow_blank && RaceStateMachine.arrow_shooted)
            {
                uprintf("RSM|Arrow blank! Going to arrow frame...\r\n");
                RaceStateMachine.state = RS_GOING_TO_ARROW_FRAME_FROM_SHOOT_POINT1;
                RudderChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
                RudderChassis.base->Go2PointStatus.target_point.x = -10.94;
                RudderChassis.base->Go2PointStatus.target_point.y = ArrowFramePoint1.y;
                RudderChassis.base->Go2PointStatus.target_yaw = 1.5708;
                RudderChassis.base->Go2PointStatus.disable_yaw_ctrl = 0;
                RudderChassis.base->Go2PointStatus.enable_always_yaw_ctrl = 1;
                RudderChassis.base->Go2PointStatus.start_speed = 0.5;
                RudderChassis.base->Go2PointStatus.final_speed = 0;
                RudderChassis.base->Go2PointStatus.start = 1;
                RudderChassis.base->Go2PointStatus.enable = 1;
                RaceStateMachine.arrived_shoot_point1 = 0;
            }
        }
        break;
    // ------------------------------------------------------------
    case RS_GOING_TO_SHOOT_POINT2:
        if (!RaceStateMachine.arrived_shoot_point2)
        {
            if (RudderChassis.base->Go2PointStatus.arrived) // 瞬态，射箭点2只可能从箭架前来，无需考虑track的情况
            {
                uprintf("RSM|Arrived at shoot point1.\r\n");
                RaceStateMachine.arrived_shoot_point2 = 1;
                Chassis_Go2Point_Reset();
                RaceStateMachine.shoot_point_arrived_count++;
            }
        }
        else // 常态，到达射箭点2
        {
            // 等待箭空且最后一只箭已射出
            if (RaceStateMachine.arrow_blank && RaceStateMachine.arrow_shooted)
            {
                uprintf("RSM|Arrow blank! Going to arrow frame...\r\n");
                RaceStateMachine.state = RS_GOING_TO_ARROW_FRAME_FROM_SHOOT_POINT2;
                RudderChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
                RudderChassis.base->Go2PointStatus.target_point.x = -10.94;
                RudderChassis.base->Go2PointStatus.target_point.y = ArrowFramePoint1.y;
                RudderChassis.base->Go2PointStatus.target_yaw = 1.5708;
                RudderChassis.base->Go2PointStatus.disable_yaw_ctrl = 0;
                RudderChassis.base->Go2PointStatus.enable_always_yaw_ctrl = 1;
                RudderChassis.base->Go2PointStatus.start_speed = 0.5;
                RudderChassis.base->Go2PointStatus.final_speed = 0;
                RudderChassis.base->Go2PointStatus.start = 1;
                RudderChassis.base->Go2PointStatus.enable = 1;
                RaceStateMachine.arrived_shoot_point1 = 0;
            }
        }
        break;
    // ------------------------------------------------------------
    case RS_GOING_TO_ARROW_FRAME_FROM_SHOOT_POINT1:
        RaceTask_JudgeHittedWall();
        if (!RaceStateMachine.arrived_arrow_frame_point1)
        {
            // 阻塞等待到达
            if (RudderChassis.base->Go2PointStatus.arrived) // 从射箭点1到箭架点1是纯跑点模式
            {
                OSLIB_Uart_Printf(&huart1, "RSM|arrived at arrow frame p1...");
                RaceStateMachine.arrived_arrow_frame_point1 = 1; // 跳转进else

                // 获取激光修正量
                float laser_delta_y = (ArrowFramePoint1_LaserDist - laser_get_dist(chassis_laser1)) / 1000.0f;
                RudderChassis.base->PostureStatus.pos_corr_y = -laser_delta_y;
                uprintf("RSM|laser_delta_y:%.5f \r\n", laser_delta_y);

                Chassis_Go2Point_Reset();
                RaceStateMachine.target_point_index = RKP_ARROW_FRAME2;
                RudderChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT; // 以纯跑点模式前往箭架点2
                RudderChassis.base->Go2PointStatus.start_speed = 0.4;
                RudderChassis.base->Go2PointStatus.final_speed = 0;
                RudderChassis.base->Go2PointStatus.disable_yaw_ctrl = 1;
                RudderChassis.base->Go2PointStatus.enable_always_yaw_ctrl = 1;
                RudderChassis.base->Go2PointStatus.target_point.x = ArrowFramePoint2.x;
                RudderChassis.base->Go2PointStatus.target_point.y = ArrowFramePoint2.y;
                RudderChassis.base->Go2PointStatus.target_yaw = 1.5708;
                RudderChassis.base->Go2PointStatus.start = 1;
                RudderChassis.base->Go2PointStatus.enable = 1;
            }
        }
        else
        {
            // 阻塞等待到达
            if (RudderChassis.base->Go2PointStatus.arrived) // 瞬态，到达箭架点2
            {
                OSLIB_Uart_Printf(&huart1, "RSM|arrived at arrow frame p2...\r\n");
                Chassis_Go2Point_Reset();
                RaceStateMachine.hitted_wall = 0;
                RaceStateMachine.arrived_arrow_frame_point1 = 0;
                RaceStateMachine.arrived_arrow_frame_point2 = 1;
                // RaceStateMachine.target_point_index = RKP_SHOOT_POINT1;
                RaceStateMachine.state = RS_FETCHING_ARROW;
                uprintf("RSM|aser_dis:%.4f\r\n", laser_get_dist(chassis_laser1));
                Signal_StartTakeArrow(); // 调用取箭函数
            }
        }
        break;
    // ------------------------------------------------------------
    case RS_GOING_TO_ARROW_FRAME_FROM_SHOOT_POINT2:
        RaceTask_JudgeHittedWall();
        if (!RaceStateMachine.arrived_arrow_frame_point1)
        {
            // 阻塞等待到达
            if (RudderChassis.base->Go2PointStatus.arrived) // 从射箭点2到箭架点1是纯跑点模式
            {
                OSLIB_Uart_Printf(&huart1, "RSM|arrived at arrow frame p1...");
                RaceStateMachine.arrived_arrow_frame_point1 = 1; // 跳转进else

                // 获取激光修正量
                float laser_delta_y = (ArrowFramePoint1_LaserDist - laser_get_dist(chassis_laser1)) / 1000.0f;
                RudderChassis.base->PostureStatus.pos_corr_y = -laser_delta_y;
                uprintf("RSM|laser_delta_y:%.5f \r\n", laser_delta_y);

                Chassis_Go2Point_Reset();
                RaceStateMachine.target_point_index = RKP_ARROW_FRAME2;
                RudderChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT; // 以纯跑点模式前往箭架点2
                RudderChassis.base->Go2PointStatus.start_speed = 0.4;
                RudderChassis.base->Go2PointStatus.final_speed = 0;
                RudderChassis.base->Go2PointStatus.disable_yaw_ctrl = 1;
                RudderChassis.base->Go2PointStatus.enable_always_yaw_ctrl = 1;
                RudderChassis.base->Go2PointStatus.target_point.x = ArrowFramePoint2.x;
                RudderChassis.base->Go2PointStatus.target_point.y = ArrowFramePoint2.y;
                RudderChassis.base->Go2PointStatus.target_yaw = 1.5708;
                RudderChassis.base->Go2PointStatus.start = 1;
                RudderChassis.base->Go2PointStatus.enable = 1;
            }
        }
        else
        {
            // 阻塞等待到达
            if (RudderChassis.base->Go2PointStatus.arrived) // 瞬态，到达箭架点2
            {
                OSLIB_Uart_Printf(&huart1, "RSM|arrived at arrow frame p2...\r\n");
                Chassis_Go2Point_Reset();
                RaceStateMachine.hitted_wall = 0;
                RaceStateMachine.arrived_arrow_frame_point1 = 0;
                RaceStateMachine.arrived_arrow_frame_point2 = 1;
                RaceStateMachine.state = RS_FETCHING_ARROW;
                uprintf("RSM|aser_dis:%.4f\r\n", laser_get_dist(chassis_laser1));
                Signal_StartTakeArrow(); // 调用取箭函数
            }
        }
        break;
    // ------------------------------------------------------------
    default:
        break;
    }
}

/**
 * @brief 判断撞墙,获取x方向修正量
 * 
 */
void RaceTask_JudgeHittedWall(void)
{
    if (!RaceStateMachine.hitted_wall)
    {
        RudderChassis.base->Go2PointStatus.min_speed = 0.2; // 补偿速度，硬怼
        if (fabs(RudderChassis.base->Go2PointStatus.target_point.x - RudderChassis.base->PostureStatus.x) >= 0.02 &&
            fabs(RudderChassis.base->PostureStatus.speed_x) < 0.0005)
        {
            RaceStateMachine.hitted_wall = 1;
            RudderChassis.base->Go2PointStatus.min_speed = 0;
            float delta_x = ArrowFramePoint2.x - RudderChassis.base->PostureStatus.x;
            RudderChassis.base->PostureStatus.pos_corr_x = delta_x;
            // 由于怼墙，可能认为一直没有到达终点，需要强制结束
            if (RudderChassis.base->ctrl_mode == CTRL_MODE_TRACK)
            {
                RudderChassis.base->TrackStatus.finished = 1;
            }
            else if (RudderChassis.base->ctrl_mode == CTRL_MODE_GO_TO_POINT)
            {
                RudderChassis.base->Go2PointStatus.arrived = 1;
                RudderChassis.base->Go2PointStatus.enable = 0;
            }
            // 获取激光修正量
            // float laser_delta_x = Laser_Array[LASER_L1].distance - FETCH_ARROW_LASER_POS_X;
            uprintf("hitted the wall! delta_x :%.4f\r\n", delta_x);
        }
    }
}

/**
 * @brief 与freeRTOS.c中的任务句柄关联
 * 
 * @param argument 
 */
void RaceTaskFunc(void *argument)
{
    RaceTasks_StateMachine_Init();
    for (;;)
    {
        RaceTacks_StateMachine_Exe();
        osDelay(10);
    }
}